/**
 ******************************************************************************
 * @FilePath: \Hero\App\Mode_SW.c
 * @Brief: 
 * @Date: 2021-04-18 15:35:44
 * @Author: Rio
 ******************************************************************************
 */

/**
 ******************************************************************************
  remote:
          up  |up  |  G_ENCODE/S_NORMAL
              |mid |  G_MANUAL
              |down|  G_VISION
          mid |up  |  C_CLAW
              |mid |  C_FOLLOW
              |down|  C_ROTATE
          down|up  |  keyboard
              |mid |  keyboard
              |down| security_mode
  keyboard:
              keyF|
              keyR|
              keyC|
 ******************************************************************************
 */

#include "Mode_SW.h"
#include "remoteCtrl.h"
#include "Motor_control.h"
#include "bsp_can.h"
#include "Chassis.h"

extern remoteCtrl_t s_rcData;
roboSetting_t roboSet;

extern motorData_t s_gimbal_pit_motor;
extern chassisMove_t s_chassisMove;
extern wl2data gimbPitDeg, capVol;
uint8_t cBoard_txBuff[8];
static void UI_API(void)
{
  gimbPitDeg.d = (int16_t) (s_gimbal_pit_motor.gyro_angle*1000);
  cBoard_txBuff[0] = gimbPitDeg.c[0];
  cBoard_txBuff[1] = gimbPitDeg.c[1];
  capVol.d = (int16_t) (s_chassisMove.capVol*1000);
  cBoard_txBuff[2] = capVol.c[0];
  cBoard_txBuff[3] = capVol.c[1];
  cBoard_txBuff[4] = roboSet.e_chassisMode;
  cBoard_txBuff[5] = roboSet.e_aimingMode;
  cBoard_txBuff[6] = roboSet.e_shootMode;
  cBoard_txBuff[7] = 0;
  cBoard_txBuff[7] = 0;
  CAN_Send_bytes(&hcan2, 0x300, cBoard_txBuff);
}

void ModeInit(void)
{
  roboSet.e_aimingMode = G_MANUAL;
  roboSet.e_chassisMode = C_FOLLOW;
  roboSet.e_shootMode = S_STOP;
}

void ModeSwitch(void)
{
  UI_API();
  static int8_t keyLock_PressL, keyLock_PressR = 0;
	static int8_t keyLock_E, keyLock_R, keyLock_CTRL, keyLock_F, keyLock_B = 0;

  if(s_rcData.rc.s[1]==2)
  {//比赛键鼠模式
    //安全模式 down | down
    if((s_rcData.rc.s[1]==2) && (s_rcData.rc.s[0]==2))
    {
      roboSet.e_aimingMode = G_NULL;
      roboSet.e_chassisMode = C_NULL;
      roboSet.e_shootMode = S_NULL;
    }
    else if((s_rcData.rc.s[1]==2) && (s_rcData.rc.s[0]==3))
    {
      roboSet.e_aimingMode = G_MANUAL;
      roboSet.e_chassisMode = C_FOLLOW;
      roboSet.e_shootMode = S_STOP;
    }
    else if((s_rcData.rc.s[1]==2) && (s_rcData.rc.s[0]==1))
    {
      if((s_rcData.rc.s[1]==2)&&((s_rcData.rc.s[0]==1)||(s_rcData.rc.s[0]==3)))
      {
        //左键 拨盘目标增
        if(s_rcData.mouse.pressLeft) keyLock_PressL=1;
          if((!s_rcData.mouse.pressLeft) && keyLock_PressL==1)
          {
            roboSet.shootOnce = 1;
            keyLock_PressL = 0;
          }
				//B键锁 回头
        if(s_rcData.key.v & KEY_PRESSED_OFFSET_B) keyLock_B=1;
          if((!(s_rcData.key.v & KEY_PRESSED_OFFSET_B)) && keyLock_B==1)
          {
            if(roboSet.e_aimingMode == G_ENCODE) {
																								  roboSet.e_aimingMode = G_MANUAL;
                                                  roboSet.turnBack++;
																									roboSet.e_chassisMode = C_FOLLOW_BACK;
						}
            else {
							roboSet.turnBack = 0;
                  roboSet.e_chassisMode = C_FOLLOW;}
            keyLock_B = 0;
          }					
        //Q键锁 自瞄打哨兵
        if(s_rcData.key.v & KEY_PRESSED_OFFSET_Q) keyLock_PressR=1;
          if((!(s_rcData.key.v & KEY_PRESSED_OFFSET_Q)) && keyLock_PressR==1)
          {
            if(roboSet.e_aimingMode == G_VISION) {
              roboSet.e_aimingMode = G_MANUAL;
              roboSet.e_chassisMode = C_FOLLOW;}
            else {roboSet.e_aimingMode = G_VISION;
                  roboSet.e_chassisMode = C_CLAW;}
            keyLock_PressR = 0;
          }
        //R键锁 陀螺
        if(s_rcData.key.v & KEY_PRESSED_OFFSET_R) keyLock_R=1;
          if(!(s_rcData.key.v & KEY_PRESSED_OFFSET_R) && keyLock_R==1)
          {
            if(roboSet.e_chassisMode == C_ROTATE) {roboSet.e_chassisMode = C_FOLLOW;}
            else {roboSet.e_chassisMode = C_ROTATE;}
            keyLock_R = 0;
          }
        //E键锁 扭屁股
        if(s_rcData.key.v & KEY_PRESSED_OFFSET_E) keyLock_E=1;
          if(!(s_rcData.key.v & KEY_PRESSED_OFFSET_E) && keyLock_E==1)
          {
            if(roboSet.e_chassisMode == C_SWING) {roboSet.e_chassisMode = C_FOLLOW; \
                                                    roboSet.e_aimingMode = G_MANUAL;}
            else {roboSet.e_chassisMode = C_SWING; \
                  roboSet.e_aimingMode = G_MANUAL;}
            keyLock_E = 0;
          }
        //Ctrl键锁 吊射
        if(s_rcData.key.v & KEY_PRESSED_OFFSET_CTRL) keyLock_CTRL=1;
          if(!(s_rcData.key.v & KEY_PRESSED_OFFSET_CTRL) && keyLock_CTRL==1)
          {
            if(roboSet.e_aimingMode == G_ENCODE) {roboSet.e_aimingMode = G_MANUAL;\
                                                  roboSet.e_chassisMode = C_FOLLOW;\
                                                  roboSet.e_shootMode = S_STOP;}
            else {roboSet.e_aimingMode = G_ENCODE;\
                  roboSet.e_chassisMode = C_CLAW;
                  roboSet.e_shootMode = S_NORMAL;}
            keyLock_CTRL = 0;
          }
        //F键锁 射击开摩擦轮
        if(s_rcData.key.v & KEY_PRESSED_OFFSET_F) keyLock_F=1;
          if(!(s_rcData.key.v & KEY_PRESSED_OFFSET_F) && keyLock_F==1)
          {
            if(roboSet.e_shootMode == S_STOP){
                                              roboSet.e_shootMode = S_NORMAL;}
            else {
                  roboSet.e_shootMode = S_STOP;}
            keyLock_F = 0;
          }
      }
    }
  }
  else
  {//调试遥控器模式
    if(s_rcData.rc.s[1]==1)
    {
      if((s_rcData.rc.s[1]==1) && (s_rcData.rc.s[0]==1)) {roboSet.e_aimingMode = G_ENCODE;\
                                                          roboSet.e_shootMode  = S_NORMAL;\
                                                          roboSet.e_chassisMode = C_CLAW;}
      // else roboSet.e_shootMode = S_STOP;
      if((s_rcData.rc.s[1]==1) && (s_rcData.rc.s[0]==3)) {roboSet.e_aimingMode = G_MANUAL;\
                                                          roboSet.e_chassisMode = C_FOLLOW;}
      if((s_rcData.rc.s[1]==1) && (s_rcData.rc.s[0]==2)) roboSet.e_aimingMode = G_VISION;
    }
    if(s_rcData.rc.s[1]==3)
    {
      if((s_rcData.rc.s[1]==3) && (s_rcData.rc.s[0]==1)) {roboSet.e_chassisMode = C_SWING;\
                                                          roboSet.e_aimingMode = G_MANUAL;}
      if((s_rcData.rc.s[1]==3) && (s_rcData.rc.s[0]==3)) {roboSet.e_chassisMode = C_FOLLOW;\
                                                          roboSet.e_aimingMode = G_MANUAL;}
      if((s_rcData.rc.s[1]==3) && (s_rcData.rc.s[0]==2)) {roboSet.e_chassisMode = C_ROTATE;\
                                                          roboSet.e_aimingMode = G_MANUAL;}
    }
  }
}


